// MotorControl.h
#ifndef MOTOR_CONTROL_H
#define MOTOR_CONTROL_H

#include <atomic>
#include <chrono>
#include <cstdint>
#include <memory>

#include "can_open_driver.h"

enum class MotorType {
  FRONT_LEFT_POWER_MOTOR,
  FRONT_LEFT_STEERING_MOTOR,
  FRONT_RIGHT_POWER_MOTOR,
  FRONT_RIGHT_STEERING_MOTOR,
  REAR_LEFT_POWER_MOTOR,
  REAR_LEFT_STEERING_MOTOR,
  REAR_RIGHT_POWER_MOTOR,
  REAR_RIGHT_STEERING_MOTOR,
};

enum class WorkMode : int8_t {
  POSITION_MODE = 1,
  SPEED_MODE = 3,
  ORIGINAL_MODE = 6,
};

class MotorController {
 public:
  MotorController(uint8_t id, std::shared_ptr<CANopenDriver> driver)
      : m_motorId(id), m_driver(driver) {}
  ~MotorController() { m_running = false; };

  virtual void init() = 0;
  virtual void powerOnSelfCheck() = 0;
  double getLinear() const;
  double getAngular() const;
  void setTargetVelocity(double linear);
  double getCurrentVelocity() const;
  void setAbsPosition(double angular);
  double getCurrentPosition() const;
  virtual void timer() = 0;
  virtual void stopMotor() = 0;

  uint8_t m_motorId{0};
  std::shared_ptr<CANopenDriver> m_driver;
  std::atomic<double> m_angular{0.0};
  std::atomic<double> m_linear{0.0};
  std::atomic<double> m_currentAngular{0.0};
  std::atomic<double> m_currentLinear{0.0};
  std::atomic<bool> m_running{false};
  std::atomic<bool> m_commandReceived{false};
  std::chrono::steady_clock::time_point m_commandReceivedTime;
  std::atomic<int32_t> m_position{0};

 protected:
  void setAbsPositionFunc();
  void setTargetVelocityFunc();
  void motorReboot();
};

class PowerMotorController : public MotorController {
 public:
  PowerMotorController(uint8_t id, std::shared_ptr<CANopenDriver>& driver)
      : MotorController(id, driver) {}
  ~PowerMotorController() = default;

  void init() final;
  void powerOnSelfCheck() final;
  void timer() final;
  void stopMotor() final;

 private:
  void updateVelocity();
};

class SteeringMotorController : public MotorController {
 public:
  SteeringMotorController(uint8_t id, std::shared_ptr<CANopenDriver>& driver)
      : MotorController(id, driver) {}
  ~SteeringMotorController() = default;

  void init() final;
  void powerOnSelfCheck() final;
  void timer() final;
  void stopMotor() final;

 private:
  void updatePosition();
};

#endif